package com.example.gyroscopeprotocol;

public class Calculate {

    /**
     * '姿态角' 计算方法：
     * 滚转角（X 轴） Roll(float) = (float) ( (int16_t) (RollH<<8) | RollL ) / 32768 * 180 (°)
     * 俯仰角（Y 轴） Pitch(float) = (float) ( (int16_t) (PitchH<<8) | PitchL ) / 32768 * 180 (°)
     * 偏航角（Z 轴） Yaw(float) = (float) ( (int16_t) (YawH<<8) | YawL ) / 32768 * 180 (°)
     */
    public static void attitudeAngle(byte[] data) {
        int RollL = data[5];
        int RollH = data[6];
        int PitchL = data[7];
        int PitchH = data[8];
        int YawL = data[9];
        int YawH = data[10];

        float Roll = (float) (((RollH << 8) | RollL)) / 32768 * 180;
        float Pitch = (float) (((PitchH << 8) | PitchL)) / 32768 * 180;
        float Yaw = (float) (((YawH << 8) | YawL)) / 32768 * 180;
    }

    private static void a(byte[] data) {
        /**
         * q0(float) = (float) ( (int16_t) (Q0H<<8) | Q0L) / 32768 （范围 0.0~1.0）
         * q1(float) = (float) ( (int16_t) (Q1H<<8) | Q1L) / 32768 （范围 0.0~1.0）
         * q2(float) = (float) ( (int16_t) (Q2H<<8) | Q2L) / 32768 （范围 0.0~1.0）
         * q3(float) = (float) ( (int16_t) (Q3H<<8) | Q3L) / 32768 （范围 0.0~1.0）
         */
        int Q0L = data[5];
        int Q0H = data[6];
        int Q1L = data[7];
        int Q1H = data[8];
        int Q2L = data[9];
        int Q2H = data[10];
        int Q3L = data[11];
        int Q3H = data[12];
        float q0 = (float) ((Q0H << 8) | Q0L) / 32768;
        float q1 = (float) ((Q1H << 8) | Q1L) / 32768;
        float q2 = (float) ((Q2H << 8) | Q2L) / 32768;
        float q3 = (float) ((Q3H << 8) | Q3L) / 32768;
    }
}
